#include "collide.h"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;

sm_cdr_result moveRectWithCdr(const CollisionMap& colMap,
                              const SDL_Rect& inPosBegin,
                              const SDL_Rect& inPosEnd,
                              SDL_Rect& outPosFinal)
{
  cout << "CDR: ******************* BEGIN ************************" << endl;
  // if collMap.map is NULL, return fatal failure
  if(colMap.map == NULL) {
    cerr << "Error!  map pointer is NULL in moveRectWithCdr()" << endl;
    return sm_cdr_failure;
  }

  cout << "CDR: inPosBegin = " << inPosBegin.x << "," << inPosBegin.y << "," << inPosBegin.w << "," << inPosBegin.h << endl;
  cout << "CDR: inPosEnd = " << inPosEnd.x << "," << inPosEnd.y << "," << inPosEnd.w << "," << inPosEnd.h << endl;

  // 1) determine scale factor from colmap to player rects 
  //    (based on colMap.cellInPixels)
  SDL_Rect scaledPosBegin, scaledPosEnd;
  scaledPosBegin.x = inPosBegin.x / colMap.cellInPixels;
  scaledPosBegin.y = inPosBegin.y / colMap.cellInPixels;
  scaledPosBegin.w = inPosBegin.w / colMap.cellInPixels;
  scaledPosBegin.h = inPosBegin.h / colMap.cellInPixels;
  if(scaledPosBegin.w == 0) { scaledPosBegin.w = 1; }
  if(scaledPosBegin.h == 0) { scaledPosBegin.h = 1; }
  cout << "CDR: scaledPosBegin = " << scaledPosBegin.x << "," << scaledPosBegin.y << "," << scaledPosBegin.w << "," << scaledPosBegin.h << endl;

  scaledPosEnd.x = inPosEnd.x / colMap.cellInPixels;
  scaledPosEnd.y = inPosEnd.y / colMap.cellInPixels;
  scaledPosEnd.w = scaledPosBegin.w;
  scaledPosEnd.h = scaledPosBegin.h;
  cout << "CDR: scaledPosEnd = " << scaledPosEnd.x << "," << scaledPosEnd.y << "," << scaledPosEnd.w << "," << scaledPosEnd.h << endl;

  outPosFinal.w = inPosBegin.w;
  outPosFinal.h = inPosBegin.h;

  // 2) with scale conversion, if begin == end position, then populate
  //    final with end pos and return success immediately
  if(scaledPosBegin.x == scaledPosEnd.x && scaledPosBegin.y == scaledPosEnd.y) {
    cout << "CDR: Did not move out of scaled squares, therefore movement can be completed" << endl;
    outPosFinal.x = inPosEnd.x;
    outPosFinal.y = inPosEnd.y;
    return sm_cdr_success;
  }

  // otherwise, we go on...

  // 3) compare x/y in inPosBegin to inPosEnd
  //    a) determine if it was a horizontal or vertical
  //    b) otherwise, error

  // these two variables describe how many strips and how many cells are involved with the movement
  int numStrips = 0;
  int numCellsInStrip = 0;

  // this is the current index we're looking at
  int currentCellIx = 0;

  // these two variables are set to the values we need to increment to get
  // to the next strip and get to the next cell
  // for instance, with upward motion, nextStrip would be set to -rowInc and nextCell would be +1
  // with rightward motion, nextStrip would be set to +1 and nextCell would be +rowInc
  int nextStrip = 0;
  int nextCell = 0;

  // these two variables hold the amounts that our new position is relative
  // to the character position 
  int deltaPosX = 0;
  int deltaPosY = 0;
  if(inPosBegin.x != inPosEnd.x && inPosBegin.y == inPosEnd.y) {
    cout << "CDR: Horizontal movement" << endl;
    
    // 4) determine how many cells are in a "strip"
    numCellsInStrip = scaledPosBegin.h;

    if(scaledPosEnd.x > scaledPosBegin.x) { 
      cout << "CDR: Right movement" << endl;
      // 5) determine how many strips we must compare from edge of bounds
      numStrips = scaledPosEnd.x - scaledPosBegin.x;
      cout << "CDR: Num strips=" << numStrips << ", cells-in-strip=" << numCellsInStrip << endl;

      nextStrip = 1;
      nextCell = colMap.width;
      currentCellIx = scaledPosBegin.x + (scaledPosBegin.y*colMap.width) + scaledPosBegin.w;
      cout << "CDR: nextStrip=" << nextStrip << ", nextCell=" << nextCell << ", currentCellIx=" << currentCellIx << endl;

      deltaPosX = (scaledPosBegin.w-1)*colMap.cellInPixels;
      deltaPosY = 0;
      cout << "CDR: delta move = " << deltaPosX << "," << deltaPosY << endl;
    }
    else {
      cout << "CDR: Left movement" << endl;
      // 5) determine how many strips we must compare from edge of bounds
      numStrips = scaledPosBegin.x - scaledPosEnd.x;
      cout << "CDR: Num strips=" << numStrips << ", cells-in-strip=" << numCellsInStrip << endl;

      nextStrip = -1;
      nextCell = colMap.width;
      currentCellIx = scaledPosBegin.x + (scaledPosBegin.y*colMap.width) - 1;

      cout << "CDR: nextStrip=" << nextStrip << ", nextCell=" << nextCell << ", currentCellIx=" << currentCellIx << endl;

      deltaPosX = -(scaledPosBegin.w-1)*colMap.cellInPixels;
      deltaPosY = 0;
      cout << "CDR: delta move = " << deltaPosX << "," << deltaPosY << endl;
    }

  }
  else if (inPosBegin.x == inPosEnd.x && inPosBegin.y != inPosEnd.y) {
    cout << "CDR: Vertical movement" << endl;
    numCellsInStrip = scaledPosBegin.w;

    if(scaledPosEnd.y > scaledPosBegin.y) { 
      cout << "CDR: Down movement" << endl;
      // 5) determine how many strips we must compare from edge of bounds
      numStrips = scaledPosEnd.y - scaledPosBegin.y;
      cout << "CDR: Num strips=" << numStrips << ", cells-in-strip=" << numCellsInStrip << endl;

      nextStrip = colMap.width;
      nextCell = 1;
      currentCellIx = scaledPosBegin.x + (scaledPosBegin.y*colMap.width) + (scaledPosBegin.h*colMap.width);

      cout << "CDR: nextStrip=" << nextStrip << ", nextCell=" << nextCell << ", currentCellIx=" << currentCellIx << endl;

      deltaPosX = 0;
      deltaPosY = (scaledPosBegin.h-1)*colMap.cellInPixels;
      cout << "CDR: delta move = " << deltaPosX << "," << deltaPosY << endl;
    }
    else {
      cout << "CDR: Up movement" << endl;
      // 5) determine how many strips we must compare from edge of bounds
      numStrips = scaledPosBegin.y - scaledPosEnd.y;
      cout << "CDR: Num strips=" << numStrips << ", cells-in-strip=" << numCellsInStrip << endl;

      nextStrip = -colMap.width;
      nextCell = 1;
      currentCellIx = scaledPosBegin.x + (scaledPosBegin.y*colMap.width) - colMap.width;

      cout << "CDR: nextStrip=" << nextStrip << ", nextCell=" << nextCell << ", currentCellIx=" << currentCellIx << endl;

      deltaPosX = 0;
      deltaPosY = -(scaledPosBegin.h-1)*colMap.cellInPixels;
      cout << "CDR: delta move = " << deltaPosX << "," << deltaPosY << endl;
    }
  }
  else {
    cerr << "Error!  Non-grid aligned movement not allowed in moveRectWithCdr()" << endl;
    return sm_cdr_failure;
  }

  // 6) start a loop through each strip, within each strip, compare each
  //    cell within that strip
  //    if we've reached the edge of the map, then adjust player to edge of map and return
  //    if we've reached a '1' in the map, then adjust player to the no-go square and return

  outPosFinal.x = inPosBegin.x;
  outPosFinal.y = inPosBegin.y;
  bool bStop = false;
  sm_cdr_result result = sm_cdr_success;
  int startOfStrip = currentCellIx;
  for(int stripnum = 0; stripnum < numStrips; ++stripnum) {
    for(int cellnum = 0; cellnum < numCellsInStrip; ++cellnum) {
      cout << "CDR: Looking at strip #" << stripnum << ", cell# " << cellnum << ", currentCellIx=" << currentCellIx << " and map value is " << (int)colMap.map[currentCellIx] << endl;
      if(colMap.map[currentCellIx]) {
        cout << "Found a 1" << endl;
        bStop = true;
        result = sm_cdr_partial_move;
        break;
      }
      currentCellIx += nextCell;
    }
    if(bStop) { 
      break; 
    }
    // now reset our current cell to calculate the (intermediate) final pos
    currentCellIx = startOfStrip;

    if(deltaPosX) {
      outPosFinal.x = (currentCellIx % colMap.width)*colMap.cellInPixels;
    }
    if(deltaPosY) {
      outPosFinal.x = (currentCellIx / colMap.width)*colMap.cellInPixels;
    }

    // now advance our currentCellIx and tracker for the start of the strip
    currentCellIx += nextStrip;
    startOfStrip = currentCellIx;
  }

  // if we didn't bump into anything, set the final pos to the end pos
  if(!bStop) {
    outPosFinal.x = inPosEnd.x;
    outPosFinal.y = inPosEnd.y;
  }
  else {
    // here is where we have to figure out where the final position is because we've
    // bumped into something along the way...
    cout << "CDR: ++++++++++++++++++++++++ HUZZAH! ++++++++++++++++++++++++++" << endl;
  }

  cout << "CDR: outPosFinal = " << outPosFinal.x << "," << outPosFinal.y << "," << outPosFinal.w << "," << outPosFinal.h << endl;

  return result;
}

